"""
通过实例化threading.Thread类创建线程
"""
import json
import time
import threading
import socketio
import roslibpy

# 用于subscribe,返回FlaskServer
messageFromDiegoTalker = []


def connect_thread_a():
    """线程运行函数"""
    sio = socketio.Client()
    sio.connect('http://127.0.0.1:8000', namespaces="/channel_A")

    # 和服务端通信
    class MyCustomClientNamespace(socketio.ClientNamespace):

        def on_connect(self):
            print('****on_connect***I \'m client  connect to Server')
            pass

        def on_disconnect(self):
            pass

        def on_diegoserver(self, *data):
            print('**************** 1.I received a message from the diego server')
            flag = True
            # <class 'tuple'>
            print(data)
            print(data[0])
            print(data[0][0])
            s1 = json.dumps(data[0][0])
            print(s1)
            if json.loads(s1)['name'] == '/cmd_vel' and json.loads(s1)['messageType'] == 'geometry_msgs/Twist':
                talker = DiegoTalker('/cmd_vel', 'geometry_msgs/Twist')
                s2 = json.dumps(data[0][1])
                talker.talk(json.loads(s2))
            if json.loads(s1)['name'] == '/listener' and json.loads(s1)['messageType'] == 'geometry_msgs/Twist':
                talker = DiegoTalker('/listener', 'geometry_msgs/Twist')
                talker.listen()
                while flag:
                    if messageFromDiegoTalker:
                        print("***************[messageFromDiegoTalker]*********before" + str(messageFromDiegoTalker))
                        # 只获取数组最新的对象[-1]，发送给FlaskServer
                        self.emit('rosbridge', messageFromDiegoTalker[-1], namespace='/channel_A')
                        flag = False
                        print("***************[messageFromDiegoTalker]*********after" + str(messageFromDiegoTalker))

    # # 注意:Flask-Socketio用on_namespace;Python-Socketio用register_namespace
    sio.register_namespace(MyCustomClientNamespace('/channel_A'))


client = roslibpy.Ros(host='192.168.1.120', port=9090)
client.run()
client.on_ready(lambda: print('Is ROS connected?', client.is_connected))


def callbackMessage(message):
    print('******************* I AM IN MESSAGE*******************')
    print(message)
    print(type(message))
    print("*********************************************")
    messageFromDiegoTalker.append(message)


# 和ros通信
class DiegoTalker:
    def __init__(self, topic, topic_type):
        self.topic = topic
        self.topic_type = topic_type

    def talk(self, data):
        talker = roslibpy.Topic(client, self.topic, self.topic_type)
        print('****************i am class DiegoTalker')
        print(data)
        print(type(data))
        talker.publish(roslibpy.Message(data))
        talker.unadvertise()

    def listen(self):
        listener = roslibpy.Topic(client, self.topic, self.topic_type)
        print('****************i am class DiegoTalker listener')
        listener.subscribe(callback=callbackMessage)
        listener.unadvertise()


def main():
    # 创建线程
    thread_hi = threading.Thread(target=connect_thread_a)
    # thread_hello = threading.Thread(target=connect_thread_b)
    # 启动线程
    thread_hi.start()
    # thread_hello.start()
    thread_hi.join()
    print('Main thread has ended!')


if __name__ == '__main__':
    main()
